#include "ctrl_protocol.h"
#include "oslib.h"

#include "cmsis_os2.h"

extern osMessageQueueId_t CtrlSendQueueHandle;

static Ctrl_Prot_t txbuf[1];
void _SendTask(void *argument)
{
    osStatus_t      ret;
    Ctrl_Prot_t *   packet = &(txbuf[0]);
    int             length;

    for (;;) {
        ret = osMessageQueueGet(CtrlSendQueueHandle, packet, NULL, osWaitForever);
        if (ret == osOK) {
            packet->pre = CTRLPROT_PRE;
            packet->src = CTRLPROT_SRC_HOST;
            packet->ticks = osKernelGetTickCount();
            length = ctrl_send_pack_append_crc(packet);
            OSLIB_Uart_SendData(&huart2, (uint8_t *)packet, length);
        }
        else if (ret == osErrorParameter) {
            Fatal("_SendTask : Error Parameter");
        }
        else if (ret == osErrorResource) {
            Error("_SendTask : Error Resource");
        }
    }    
}